#pragma warning(disable: 4819) // Disable warning for characters not representable in current code page
#include "submarine_energy_calculator.h"
#define _USE_MATH_DEFINES // For M_PI
#include <cmath> // For std::cos, std::sin, std::atan2, std::sqrt, M_PI
#include <numeric> // For std::accumulate
#include <iostream>

// Define some constants, such as water density, submarine drag coefficient, etc., simplified here
const double WATER_DENSITY = 1025.0; // Seawater density kg/m^3
const double DRAG_COEFFICIENT = 0.5; // Drag coefficient (simplified)
double REFERENCE_AREA = 10.0;  // Reference area (submarine cross-sectional area, simplified) m^2;

// Forward declarations
double calculateEffectiveDragCoefficient(double base_drag_coefficient, double reynolds_number);
double calculatePropulsionEfficiency(double speed);

// Convert angle from degrees to radians
double toRadians(double degrees) {
    return degrees * std::acos(-1.0) / 180.0; // Use std::acos(-1.0) for PI
}

// Calculate additional energy cost at a single waypoint
double calculateWaypointEnergyCost(const Waypoint& waypoint, const OceanCurrent& current) {
    // Submarine's own velocity vector (x, y components)
    double sub_vx = waypoint.speed * std::sin(toRadians(waypoint.heading));
    double sub_vy = waypoint.speed * std::cos(toRadians(waypoint.heading));

    // Calculate the actual velocity vector of UUV relative to water body (this is the fundamental cause of drag)
    // Relative velocity = UUV velocity - Ocean current velocity
    double relative_vx = sub_vx - current.uo;  // Eastward relative velocity
    double relative_vy = sub_vy - current.vo;  // Northward relative velocity
    double relative_speed = std::sqrt(relative_vx * relative_vx + relative_vy * relative_vy);

    // Calculate the angle between heading and relative flow direction (for lateral force calculation)
    // Note: Need to unify coordinate systems
    // atan2 coordinate system: 0° = East, 90° = North, 180° = West, -90° = South
    // Heading coordinate system: 0° = North, 90° = East, 180° = South, 270° = West
    
    double relative_flow_direction = std::atan2(relative_vy, relative_vx); //(-180, 180]
    double heading_angle = toRadians(waypoint.heading);  //(0, 360)
    
    // Convert heading coordinate system to atan2 coordinate system
    // Heading coordinate system: 0°(North) -> atan2 coordinate system: 90°
    // Heading coordinate system: 90°(East) -> atan2 coordinate system: 0°
    // Heading coordinate system: 180°(South) -> atan2 coordinate system: -90°
    // Heading coordinate system: 270°(West) -> atan2 coordinate system: 180°
    double heading_atan2 = M_PI/2.0 - heading_angle;
    
    // westnorth:[-180, -270) -> [180 ,90)
    if (heading_atan2 <= -M_PI) {
        heading_atan2 += 2.0 * M_PI;
    }
    
    double angle_difference = std::abs(heading_atan2 - relative_flow_direction);
    
    // Limit angle to 0-π range (take the smaller angle difference)
    if (angle_difference > M_PI) {
        angle_difference = 2.0 * M_PI - angle_difference;
    }

    // Calculate Reynolds number
    // Re = ρ * V * L / μ, L is the longth of UUV
    const double SUBMARINE_LENGTH = 50.0;
    const double WATER_VISCOSITY = 1.0e-3;
    double reynolds_number = WATER_DENSITY * relative_speed * SUBMARINE_LENGTH / WATER_VISCOSITY;

    // Calculation of drag coefficient based on Reynolds number
    double effective_drag_coefficient = calculateEffectiveDragCoefficient( DRAG_COEFFICIENT, reynolds_number);

    // Basic drag: Based on relative velocity and effective drag coefficient
    double base_drag_force = 0.5 * WATER_DENSITY * relative_speed * relative_speed * 
                             effective_drag_coefficient * REFERENCE_AREA;

    // Calculate the lateral force (when the UUV heading is not aligned with the relative current direction)
    // The lateral force coefficient increases as the angular difference increases
    double side_force_coefficient = 0.05 * std::sin(angle_difference);
    double side_force = 0.5 * WATER_DENSITY * relative_speed * relative_speed * 
                        side_force_coefficient * REFERENCE_AREA;

    // Calculate the total resistance that the propulsion system needs to overcome
    // Total drag = Base drag + Component of lateral force in the heading direction
    double total_drag_force = base_drag_force;
    
    // The component of lateral force in the heading direction (only considered when the angular difference is large enough)
    if (angle_difference > 0.01) {
        double side_force_contribution = side_force * std::sin(angle_difference);
        total_drag_force += std::abs(side_force_contribution);
    }

    // Calculate propulsion power requirement
    // Power = Drag × Speed (component in heading direction)
    // Fix: Use submarine's own speed instead of relative speed, because propulsion system needs to maintain submarine's absolute speed
    double propulsion_power = total_drag_force * waypoint.speed;

    // Consider propulsion efficiency (varies with speed)
    double propulsion_efficiency = calculatePropulsionEfficiency(waypoint.speed);

    // Actual required input power
    double required_input_power = propulsion_power / propulsion_efficiency;

    std::cout << "required_input_power, propulsion_power, propulsion_efficiency = " << required_input_power << ", "
        << propulsion_power << ", " << propulsion_efficiency << ", " << std::endl;
    // Ensure power is positive
    //return std::abs(required_input_power);
    return required_input_power;
}

// Calculat Effective Drag Coefficient
double calculateEffectiveDragCoefficient(double base_drag_coefficient, double reynolds_number) {

    double reynolds_factor = 1.0;
    if (reynolds_number < 5e5) {
        reynolds_factor = 0.8;
    } else if (reynolds_number < 1e6) {
        reynolds_factor = 1.0 + 0.2 * (reynolds_number - 5e5) / 5e5;
    } else {
        reynolds_factor = 1.2;
    }
    
    return base_drag_coefficient * reynolds_factor;
}

// Calculate propulsion efficiency (varies with speed)
double calculatePropulsionEfficiency(double speed) {
    // Propulsion efficiency is usually lower at low speeds, peaks at medium speeds, and slightly decreases at high speeds
    if (speed < 2.0) {
        // Low speed region: lower efficiency
        return 0.5 + 0.1 * speed;
    } else if (speed < 8.0) {
        // Medium speed region: higher efficiency
        return 0.7 + 0.05 * (speed - 2.0);
    } else {
        // High speed region: efficiency slightly decreases
        return 0.95 - 0.01 * (speed - 8.0);
    }
}

// The Whole Way Energy Cost
double calculateTotalEnergyCost(const std::vector<Waypoint>& waypoints, const std::vector<OceanCurrent>& currents) {
    if (waypoints.size() != currents.size() || waypoints.empty()) {
        return 0.0; // Waypoints and ocean current data do not match or are empty
    }

    double total_energy_cost = 0.0;

    for (size_t i = 0; i < waypoints.size(); ++i) {
        // Time Interval between this point with the next point
        
        double time_interval = 0.0;
        if (i < waypoints.size() - 1) {
            time_interval = waypoints[i+1].time - waypoints[i].time;
        } else if (waypoints.size() == 1) {
            time_interval = 1.0; // if only one point
        } else {
            // the last point
            time_interval = waypoints[i].time - waypoints[i-1].time;
        }
        
        if (time_interval <= 0) {
            continue;
        }

        double waypoint_energy_cost_per_second = calculateWaypointEnergyCost(waypoints[i], currents[i]);
        total_energy_cost += waypoint_energy_cost_per_second * time_interval;
    }

    return total_energy_cost;
}